#include "encoder_device.hpp"
#include "mcu_device.hpp"

#include <math.h>

namespace sros::device
{
    EncoderDevice::EncoderDevice(std::shared_ptr<McuDevice> mcu_device) 
        : device_impl<cfg::EncoderCfg>()
        , mcu_(mcu_device)
    {
    }

    EncoderDevice::~EncoderDevice()
    {
    }

    RetBool EncoderDevice::init()
    {
        // 加载配置
        auto ret = load_config();
        if (!ret.v)
        {
            return ret;
        }

        ret = config_->get(cfg_);
        if (!ret.v)
        {
            return ret;
        }

        data_[0].dir = 0;
        data_[0].pos = 0;
        data_[0].pulse = 0;

        data_[1].dir = 1;
        data_[1].pos = 0;
        data_[1].pulse = 0;

        return {true};
    }

    void EncoderDevice::run()
    {
    }

    void EncoderDevice::close()
    {
        if (config_)
        {
            config_->deinit();
            config_ = nullptr;
        }
    }

    void EncoderDevice::parse(const std::map<uint32_t, int32_t> &data)
    {
        std::lock_guard<std::mutex> lck(mtx_);

        if (cfg_.H.enable && data.find(0) != data.end())
        {
            double wheel_circumference = M_PI * (double)cfg_.H.diameter;
            double position = (data.at(0) * wheel_circumference) / ((double)cfg_.H.resolution * cfg_.H.gear_ratio);
            data_[0].pos = (int32_t)(position + 0.5);
            data_[0].pulse = data.at(0);
        }

        if (cfg_.V.enable && data.find(1) != data.end())
        {
            double wheel_circumference = M_PI * (double)cfg_.V.diameter;
            double position = (data.at(1) * wheel_circumference) / ((double)cfg_.V.resolution * cfg_.V.gear_ratio);
            data_[1].pos = (int32_t)(position + 0.5);
            data_[1].pulse = data.at(1);
        }
    }

    RetBool EncoderDevice::load_config()
    {
        if (config_)
        {
            return {true};
        }

        std::string root = util::env_var("HOME");
        if (root.empty())
        {
            root = ".";
        }
        std::string config_path = util::format("{}/.sros/{}/{}/config/encoder_config.json", root, MODULE, NODE);

        config_ = std::make_shared<commons::Config<cfg::EncoderCfg>>();
        auto ret = config_->init(config_path);
        if (!ret.v)
        {
            return ret;
        }

        return config_->load();
    }
}